/***************************************************************************
 创建者: 华磊
 开始时间: 2020.8.14
 copyright: (C) 华友高科
 修改说明: (每次有修改就添加一条，带有 修改人，修改时间，修改描述)
 example (1) hualei 2020.5.8 类的具体实现代码编写

 ***************************************************************************
 *  @file x.h
 *  功能:
 *
 *                                                                         *
 ***************************************************************************/
#ifndef DRAGCONTROL_H
#define DRAGCONTROL_H
#include <qvector.h>
#include "trajectoryudefine.h"
class FilteringAlgorithm;
class TrajectoryServer;
/**
 * @brief The DragControl class 拖动功能
 */
class DragControl
{
public:
    DragControl(QString configFilePathIn, TrajectoryServer *trajServerIn, RobotCommonParameter * roboParameterIn);

public:
    int setCollisionWarnTorque(int axisIndex,double  torqueIn);
    int setCollisionDetectRatio(double ratioIn);
    bool isInitialOk();

    int setDragPara(int axisIndex,DragPara paraIn);
    int dragLoopRealTime();
    int dragLoopCommon();
    int dragOn();
    int dragOff();

    int collisionDetectLoop();

private:
    int collisionDetectLoop_oneAxis(double torqueFeedback, double idealTorque,double warnTorqueBias);
    //阻抗控制
    int dragLoopOneAxis_soft(double torqueFeedback, double idealTorque,
                        double controlKp, double thresholdTorque, double &positionErrorOut);
    //重力补偿控制
    int dragLoopOneAxis(double torqueFeedback, double idealTorque,
                        double controlKp, double thresholdTorque, double &torqueCommandOut);
    //轨迹控制
    int dragLoopCommonSingleAxis(int jointIndex);

    int readConfigFile(QString filePathIn);

private:
    QString configFilePath;
    int robotId;
    double deltaTime;
    TrajectoryServer *trajServer;
    RobotCommonParameter *roboParameter;
    int jointNum;
    QVector<DragPara> dragParaList;

    QVector<FilteringAlgorithm*> filterAlgo;
    QVector<FilteringAlgorithm*> torquefilterAlgo;
    QVector<int> dragStatus;

    QVector<double> moveLength;
    QVector<double> idealValue;
    QVector<double> thresholdStartValue;
    QVector<double> thresholdRunValue;
    bool isInitialOkStatus;
    QVector<double> warnTorqueBiasList;
    double collisionDetectRatio;
    bool isUseThisModule;


};

#endif // DRAGCONTROL_H
